package org.usfirst.frc.team1322.robot.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotDrive;
import org.usfirst.frc.team1322.robot.RobotMap;
import org.usfirst.frc.team1322.robot.commands.JoystickDrive;

/**
 *
 */
public class DriveSystem extends Subsystem {
    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
	CANTalon LeftFront, LeftRear, RightFront, RightRear;
	RobotDrive MechDrive;
	Encoder eLeftFront;
	
	public DriveSystem(){
		LeftFront = new CANTalon(RobotMap.pLeftFront);
		LeftRear = new CANTalon(RobotMap.pLeftRear);
		RightFront = new CANTalon(RobotMap.pRightFront);
		RightRear = new CANTalon(RobotMap.pRightRear);
		MechDrive = new RobotDrive(LeftFront, LeftRear, RightFront, RightRear);
	}
	
	/*****************************************************************
	 * DRIVE ROBOT
	 * Dumb input method for drive system
	 * 
	 * TODO: Investigate using a gyro here
	 * 
	 * @param mX Double, Sets Lateral Power
	 * @param mY Double, Sets Forward Power
	 * @param mR Double, Sets Rotation Power
	 */
	public void DriveRobot(double mX, double mY, double mR){
		MechDrive.mecanumDrive_Cartesian(mX, mY, mR, 0);
	}

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        setDefaultCommand(new JoystickDrive());
    }

	public void resetEncoders() {
		// TODO Set each motor encoder to reset
		
		
	}

	public void setPos(double tLocation) {
		// TODO Set Encoders
		
	}

	public void enableClosedLoop() {
		// TODO Enable Closed Loop CAN
		
	}

	public boolean isDone() {
		// TODO Return if ClosedLoop Is done
		return false;
	}
}

